#!/usr/bin/env python3

import rospy
import math
import tf
import threading
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist
from std_msgs.msg import String
from sensor_msgs.msg import LaserScan
from nav_to_point import NavToPoint

class MapNavigator:
    """在地图坐标系中导航的控制器"""
    
    def __init__(self):
        rospy.init_node('map_navigator')
        
        # 创建TF监听器，用于获取机器人在地图中的位置
        self.tf_listener = tf.TransformListener()
        rospy.sleep(1)  # 等待TF树建立
        
        # 初始化导航控制器
        self.navigator = NavToPoint()
        
        # 订阅RViz中的导航目标点
        self.goal_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.goal_callback)
        
        # 订阅初始位姿估计
        self.init_pose_sub = rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, self.init_pose_callback)
        
        # 订阅激光扫描数据，用于避障
        self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback)
        
        # 发布状态信息
        self.status_pub = rospy.Publisher('/map_navigator/status', String, queue_size=5)
        
        # 直接发布速度指令的发布器（用于紧急停止）
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        
        # 初始化变量
        self.is_navigating = False
        self.goal_reached = False
        self.navigation_thread = None
        self.obstacle_detected = False
        self.obstacle_distances = []  # 激光扫描中的障碍物距离
        self.consecutive_avoidance_count = 0  # 连续避障次数
        self.last_avoidance_direction = 0  # 上次避障方向
        
        # 导航参数
        self.goal_position_tolerance = 0.25  # 目标位置容差，单位：米
        self.goal_angle_tolerance = 0.1  # 目标角度容差，单位：弧度
        self.obstacle_distance_threshold = 0.1  # 障碍物检测阈值，单位：米
        self.stall_detection_time = 3.0  # 停滞检测时间，单位：秒
        self.max_consecutive_avoidance = 3 # 最大连续避障次数
        
        rospy.loginfo("地图导航器已初始化，等待导航目标...")
    
    def init_pose_callback(self, msg):
        """处理RViz中设置的初始位置"""
        rospy.loginfo("接收到初始位置设置")
        # 初始位置由AMCL直接处理，这里只提供反馈
        self.status_pub.publish("已设置初始位置")
    
    def scan_callback(self, msg):
        """处理激光扫描数据，用于避障"""
        # 提取前方120度范围内的障碍物距离
        ranges = msg.ranges
        
        # 确定正前方的扫描范围 (假设激光从左到右扫描)
        total_points = len(ranges)
        center_index = total_points // 2
        angle_span = 60  # 检查前方60度范围的障碍物
        start_index = center_index - angle_span // 2
        end_index = center_index + angle_span // 2
        
        # 获取前方有效距离值 - 过滤掉0和极小值，这些可能是错误读数
        front_ranges = []
        for i in range(start_index, end_index):
            if i >= 0 and i < total_points:
                # 过滤掉0和接近0的值，以及无穷大和NaN
                if ranges[i] > 0.05 and not math.isinf(ranges[i]) and not math.isnan(ranges[i]):
                    front_ranges.append(ranges[i])
        
        # 重置障碍检测状态，只在有足够的有效数据时才确认障碍物
        self.obstacle_detected = False
        
        # 如果有足够的有效距离值，判断是否有障碍物
        if len(front_ranges) > 5:  # 确保有足够的数据点
            min_distance = min(front_ranges)
            self.obstacle_distances = front_ranges
            self.obstacle_detected = min_distance < self.obstacle_distance_threshold
            if self.obstacle_detected and self.is_navigating:
                rospy.logwarn(f"检测到障碍物，距离: {min_distance:.2f}米")
    
    def goal_callback(self, msg):
        """处理RViz中设置的导航目标"""
        if self.is_navigating:
            rospy.logwarn("导航正在进行中，取消当前导航并开始新的导航")
            self.stop_navigation()
        
        goal_x = msg.pose.position.x
        goal_y = msg.pose.position.y
        
        # 从四元数中提取目标朝向
        quaternion = (
            msg.pose.orientation.x,
            msg.pose.orientation.y,
            msg.pose.orientation.z,
            msg.pose.orientation.w
        )
        _, _, goal_yaw = tf.transformations.euler_from_quaternion(quaternion)
        
        rospy.loginfo(f"接收到地图中的导航目标: x={goal_x:.2f}, y={goal_y:.2f}, yaw={math.degrees(goal_yaw):.2f}°")
        
        # 使用线程来处理导航，以便可以监控和中断
        self.navigation_thread = threading.Thread(
            target=self.navigate_to_map_point,
            args=(goal_x, goal_y, goal_yaw)
        )
        self.navigation_thread.start()
    
    def stop_navigation(self):
        """停止当前导航任务"""
        if self.is_navigating:
            self.is_navigating = False
            # 发送停止命令
            stop_msg = Twist()
            self.cmd_vel_pub.publish(stop_msg)
            rospy.loginfo("导航已停止")
            self.status_pub.publish("导航已停止")
            
            # 确保导航控制器清理资源
            self.navigator.clean_up()
    
    def navigate_to_map_point(self, map_x, map_y, map_yaw):
        """导航到地图中的指定点位，使用连续反馈控制"""
        self.is_navigating = True
        self.goal_reached = False
        start_time = rospy.Time.now()
        last_position = None
        stall_start_time = None
        
        try:
            self.status_pub.publish("开始导航...")
            
            # 导航循环，直到到达目标或被中断
            rate = rospy.Rate(5)  # 5Hz的控制循环
            while self.is_navigating and not rospy.is_shutdown():
                try:
                    # 等待TF转换可用
                    self.tf_listener.waitForTransform("map", "base_link", rospy.Time(0), rospy.Duration(1.0))
                    
                    # 获取机器人在地图中的当前位置
                    (trans, rot) = self.tf_listener.lookupTransform("map", "base_link", rospy.Time(0))
                    
                    robot_x, robot_y = trans[0], trans[1]
                    _, _, robot_yaw = tf.transformations.euler_from_quaternion(rot)
                    
                    # 检查是否到达目标位置
                    dx = map_x - robot_x
                    dy = map_y - robot_y
                    distance_to_goal = math.sqrt(dx*dx + dy*dy)
                    
                    # 计算角度差
                    angle_to_goal = math.atan2(dy, dx)
                    angle_diff = angle_to_goal - robot_yaw
                    angle_diff = math.atan2(math.sin(angle_diff), math.cos(angle_diff))  # 规范化角度
                    
                    # 计算最终朝向差
                    final_angle_diff = map_yaw - robot_yaw
                    final_angle_diff = math.atan2(math.sin(final_angle_diff), math.cos(final_angle_diff))
                    
                    # 检查停滞状态
                    if last_position:
                        pos_change = math.sqrt((robot_x - last_position[0])**2 + (robot_y - last_position[1])**2)
                        if pos_change < 0.01:  # 如果位置变化很小
                            if stall_start_time is None:
                                stall_start_time = rospy.Time.now()
                            elif (rospy.Time.now() - stall_start_time).to_sec() > self.stall_detection_time:
                                rospy.logwarn("检测到机器人停滞，尝试重新规划路径")
                                # 可以尝试旋转一小段角度，然后继续导航
                                self.navigator.goto([0, 0, 30])  # 旋转30度
                                stall_start_time = None
                        else:
                            stall_start_time = None
                    
                    last_position = (robot_x, robot_y)
                    
                    # 记录当前状态
                    rospy.loginfo(f"当前位置: x={robot_x:.2f}, y={robot_y:.2f}, 距离目标: {distance_to_goal:.2f}米")
                    
                    # 检查是否到达目标
                    if distance_to_goal < self.goal_position_tolerance and abs(final_angle_diff) < self.goal_angle_tolerance:
                        rospy.loginfo("到达目标位置")
                        self.goal_reached = True
                        self.status_pub.publish("导航完成")
                        break
                    
                    # 处理障碍物避让
                    if self.obstacle_detected:
                        rospy.logwarn("避让障碍物")
                        self.status_pub.publish("避让障碍物")
                        
                        # 避障计数器增加，记录连续避障次数
                        self.consecutive_avoidance_count = getattr(self, 'consecutive_avoidance_count', 0) + 1
                        
                        # 如果连续多次避障，可能陷入死循环，尝试更大幅度的移动
                        if self.consecutive_avoidance_count > 5:
                            rospy.logwarn(f"连续避障{self.consecutive_avoidance_count}次，尝试更激进的避障")
                            # 尝试后退一点再转向
                            self.navigator.goto([-0.2, 0, 0])  # 后退0.2米
                            rospy.sleep(1.0)
                            
                            # 使用更大角度旋转
                            turn_angle = 60 if self.consecutive_avoidance_count % 2 == 0 else -60
                            self.navigator.goto([0, 0, turn_angle])
                            rospy.sleep(2.0)
                            
                            if self.consecutive_avoidance_count > 10:
                                # 如果避障次数过多，重置导航状态
                                rospy.logwarn("避障次数过多，放弃当前导航目标")
                                self.status_pub.publish("避障失败，放弃目标")
                                return
                        else:
                            # 正常避障逻辑
                            if self.obstacle_distances:
                                # 简单避障策略：向左或向右旋转一定角度
                                if len(self.obstacle_distances) > 2:
                                    mid_point = len(self.obstacle_distances) // 2
                                    left_dist = sum(self.obstacle_distances[:mid_point]) / mid_point
                                    right_dist = sum(self.obstacle_distances[mid_point:]) / (len(self.obstacle_distances) - mid_point)
                                    
                                    if left_dist > right_dist:  # 左侧空间较大
                                        self.navigator.goto([0, 0, 30])  # 向左转30度
                                        rospy.loginfo("左侧空间较大，向左避障")
                                    else:  # 右侧空间较大
                                        self.navigator.goto([0, 0, -30])  # 向右转30度
                                        rospy.loginfo("右侧空间较大，向右避障")
                                else:
                                    # 如果数据太少，尝试随机方向
                                    turn_direction = 30 if rospy.get_time() % 2 > 1 else -30
                                    self.navigator.goto([0, 0, turn_direction])
                        
                        # 短暂延迟以便完成转向
                        rospy.sleep(1.0)
                        continue
                    else:
                        # 如果没有检测到障碍物，重置避障计数
                        self.consecutive_avoidance_count = 0
                    
                    # 根据距离和角度差选择不同的导航策略
                    if abs(angle_diff) > 0.5:  # 如果方向偏差大于0.5弧度（约28.6度）
                        # 先调整方向
                        rotation_deg = math.degrees(angle_diff)
                        rospy.loginfo(f"调整方向: {rotation_deg:.2f}度")
                        self.navigator.goto([0, 0, rotation_deg])
                    else:
                        # 朝向已基本对齐，向目标移动
                        # 计算移动距离，但不要一次性移动过远
                        move_distance = min(distance_to_goal, 0.5)  # 最多移动0.5米
                        rospy.loginfo(f"向目标移动: {move_distance:.2f}米")
                        self.navigator.goto([move_distance, 0, 0])
                        
                        # 如果接近目标，调整最终朝向
                        if distance_to_goal < 0.3 and abs(final_angle_diff) > 0.1:
                            final_rotation_deg = math.degrees(final_angle_diff)
                            rospy.loginfo(f"调整最终朝向: {final_rotation_deg:.2f}度")
                            self.navigator.goto([0, 0, final_rotation_deg])
                
                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
                    rospy.logerr(f"TF错误: {e}")
                    continue
                    
                # 导航超时检测（5分钟）
                if (rospy.Time.now() - start_time).to_sec() > 300:
                    rospy.logwarn("导航超时，取消导航任务")
                    self.status_pub.publish("导航超时")
                    break
                
                rate.sleep()
            
            if not self.goal_reached and self.is_navigating:
                self.status_pub.publish("导航未完成")
                
        except Exception as e:
            rospy.logerr(f"导航过程中发生错误: {e}")
            self.status_pub.publish(f"导航错误: {str(e)}")
        finally:
            self.is_navigating = False

if __name__ == "__main__":
    try:
        navigator = MapNavigator()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
